/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-18     weihui       the first version
 */
#include "IMU/Imu.h"
#include "Bsp/bsp_uart4.h"
#include "Bsp/bsp_uart4_dma.h"
#include "Bsp/bsp_crc.h"
#include "Bsp/bsp_converter.h"

#define DBG_TAG "imu"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

static float_t array[3]={0};

#define CMD_HEAD 0XEE  //帧头
#define CMD_TAIL 0XFFFCFFFF //帧尾

 qsize cmd_pos = 0;  //当前指令指针位置

struct QUEUE  que;


void queue_reset()
{
    que._head = que._tail = 0;
}

void queue_push(qdata *dataS,uint16_t rx_length)
{
    for(uint16_t i =0;i<rx_length;i++)
    {
        qsize pos = (que._head+1) % QUEUE_MAX_SIZE;
        if(pos!=que._tail)//非满状态
        {
            que._data[que._head] = dataS[i];
            que._head = pos;
        }
    }
}


//从队列中取一个数据
static void queue_pop(qdata* _data)
{
    if(que._tail!=que._head)//非空状态
    {
        *_data = que._data[que._tail];
//      que._data[que._tail]=0; //协议缓存清零
        que._tail = (que._tail+1)%QUEUE_MAX_SIZE;
    }
}

//获取队列中有效数据个数
static qsize queue_size()
{
    return ((que._head+QUEUE_MAX_SIZE-que._tail)%QUEUE_MAX_SIZE);
}

qsize queue_find_cmd(qdata *rxbuff,qsize buf_len)
{
    qsize cmd_size = 0;
    qdata _data = 0;
    unsigned short rx_index=0;
    uint8_t sum=0;
    while(queue_size()>0)
    {
        //取一个数据
        queue_pop(&_data);

        if(rx_index ==0)
        {
            rxbuff[0] = rxbuff[1];
            rxbuff[1]=_data;
            if((rxbuff[0]==0x55)&&(rxbuff[1]==0x53))
            {
                  rx_index=2;
            }
        }
        else
        {
            rxbuff[rx_index++]=_data;
            if( rx_index == buf_len)     //buf_len ==11
            {
                for(uint8_t i =0;i<buf_len-1 ;i++)sum = sum + rxbuff[i];

                if (sum == rxbuff[10])//crc校验
                {
                    return rx_index;
                }
                else
                {
                    for(int i =0;i<11;i++)rt_kprintf("%02x ",rxbuff[i]);
                    rt_kprintf("\n");
                    LOG_E("crc error/n");
                    return 0;
                }
            }
        }
    }
}



/* imu接收线程入口 */
static void imu_rx_thread_entry(void* parameter)
{
    static qdata dma_rxbuff[11]={0};
    static uint16_t dma_rx_len=0;;

    static qdata cmd_buff[11]={0};
    static uint8_t cmd_len;
    queue_reset();
    while(1)
    {
        dma_rx_len = serial_dma_rx(dma_rxbuff,RT_WAITING_FOREVER);
        queue_push(dma_rxbuff,dma_rx_len);

        cmd_len = queue_find_cmd(cmd_buff,11);
        if(cmd_len)
        {
            array[0] = ((float_t)bsp_cnv_arr_to_uint16(&cmd_buff[2], false))/32768*180;
            array[1] = ((float_t)bsp_cnv_arr_to_uint16(&cmd_buff[4], false))/32768*180;
            array[2] = ((float_t)bsp_cnv_arr_to_uint16(&cmd_buff[6], false))/32768*180;
//            LOG_D("ROLL= %.3f,pitch= %.3f,yaw= %.3f\n",array[0],array[1],array[2]);
        }
    }
}

void imu_init(void)
{
#define THREAD_STACK_SIZE   1024
#define THREAD_PRIORITY     10
#define THREAD_TIMESLICE    10


    rt_thread_t tid1 = RT_NULL;

    /* 创建线程 1 */
    tid1 = rt_thread_create("imu_rx",
                            imu_rx_thread_entry, RT_NULL,
                            THREAD_STACK_SIZE,
                            THREAD_PRIORITY, THREAD_TIMESLICE);
    if (tid1 != RT_NULL)
        rt_thread_startup(tid1);

}


void get_imu_angel(float *value)
{
    rt_enter_critical();
    for(int8_t i=0;i<sizeof(array)/sizeof(float_t);i++)value[i]=array[i];
    rt_exit_critical();
}


float imu_raw_angle_clc(float initial_angle,float current_angle)
{
    float_t sum,error=0;
    error = current_angle - initial_angle;
//    if((initial_angle>0)&&(current_angle<0))
//    {
//        if(error < -180) sum = 360+current_angle-initial_angle;   //+180  - -180   //cw
////        else{
////            sum = current_angle - initial_angle;   //+90 - -90  //ccw
////        }
//    }
//    else if((initial_angle<0)&&(current_angle>0))
//    {
//        if(error >180) sum = current_angle -initial_angle-360;   //-180  - +180   //ccw
////        else{
////            sum = current_angle - initial_angle;   //-90 - +90   //cw
////        }
//    }
//    else {
//        sum = current_angle - initial_angle;
//    }
    if(error>=-360 && error<=-180)
    {
        sum = 360+error;
    }
    else if(error>=180 && error<=360)
    {
        sum = error - 360;
    }
    else {
        sum = error;
    }
//    rt_kprintf("i:%.3f,c:%.3f,s:%.3f\n",initial_angle,current_angle,sum);
    return (sum);
}
/*

Roll~Yaw
寄存器地址：61~63

发送： 50 03 00 3D 00 03 99 86(读取三轴角度)
返回： 50 03 06 RollH RollL PitchH PitchL YawH YawL crc_H crc_L

Roll[15:0]=((short)RollH << 8) | RollL;
Pitch[15:0]=((short)PitchH << 8) | PitchL;
Yaw[15:0]=((short)YawH << 8) | YawL;
 */
